﻿Imports System.Text
Imports System.ComponentModel
Imports System.Runtime.InteropServices
Imports System.Threading

'2020-09-25 考虑把读和解析数据专门放一个线程

Public Class WingPort : Inherits WebPort

    Private Const MutexWaitTimeout = 2000
    Public Enum SetOdomMethod
        Incremental = 0
        Absolute = 1
    End Enum

#Region "protocol"
    Public Shared FrameGetVersion() As Byte = New Byte(4) {&H5A, 0, &H0, &H5A, &H7E}
    Public Shared FrameResetWheels() As Byte = New Byte(4) {&H5A, 12, &H0, &H66, &H7E}
    Public Shared FrameGetRobotState() As Byte = New Byte(4) {&H5A, 11, &H0, &H65, &H7E}
    Public Shared FrameGetOdom() As Byte = New Byte(4) {&H5A, 5, &H0, &H5F, &H7E}
    Public Shared FrameClearOdom() As Byte = New Byte(4) {&H5A, 3, &H0, &H5D, &H7E}

    Public Const ID_GET_VERSION As Byte = 0
    Public Const ID_CLEAR_ODOM As Byte = 3
    Public Const ID_SET_VELOCITY As Byte = 4
    Public Const ID_GET_ODOM As Byte = 5
    Public Const ID_SET_SHIFT As Byte = 6
    Public Const ID_SET_EACH_MOTOR As Byte = 8
    Public Const ID_SET_ARM As Byte = 9
    Public Const ID_SET_RAISE As Byte = 10
    Public Const ID_GET_ROBOT_STATE As Byte = 11
    Public Const ID_RESET_WHEELS As Byte = 12
    Public Const ID_SET_SPIN As Byte = 13
    Public Const ID_SET_DOUBLE_CAR As Byte = 14
    Public Const ID_SET_ODOM_TF As Byte = 15
    Public Const ID_MESSGAE_MAX As Byte = 15

    Private ReadOnly TeamPosition As Dictionary(Of String, Integer) = New Dictionary(Of String, Integer) From {
        {"None", 0}，
        {"Front", 1},
        {"Back", 2}
    }

    Private ReadOnly AckLength As Dictionary(Of Byte, Integer) = New Dictionary(Of Byte, Integer) From {
        {ID_GET_VERSION, 32 + 5},
        {ID_CLEAR_ODOM, 5},
        {ID_SET_VELOCITY, 5},
        {ID_SET_SHIFT, 5},
        {ID_GET_ODOM, Marshal.SizeOf(Odom) + 5},
        {ID_SET_EACH_MOTOR, 5},
        {ID_SET_ARM, 5},
        {ID_SET_RAISE, 5},
        {ID_GET_ROBOT_STATE, Marshal.SizeOf(RobotState) + 5},
        {ID_RESET_WHEELS, 5},
        {ID_SET_SPIN, 5},
        {ID_SET_DOUBLE_CAR, 5},
        {ID_SET_ODOM_TF, 5}
    }

    Public Const CtrlModeMove As Integer = 0
    Public Const CtrlModeSpin As Integer = 1
    Public Const CtrlModeShift As Integer = 2
    Public Const CtrlModeNotReady As Integer = 99
    Public ReadOnly StateDict As Dictionary(Of Byte, String) = New Dictionary(Of Byte, String) From {
        {CtrlModeMove, "moving"},
        {CtrlModeSpin, "spinning"},
        {CtrlModeShift, "shifting"}
    }

#End Region

    Public EachMotor As SetEachMotor_T
    Public Arm As SetHydroArm_T
    Public CarVel As SetVelocity_T
    Public Raise As SetHydroRaise_T
    Public Spin As SetSpin_T
    Public Shift As SetShift_T
    Public RobotState As RobotState_T
    Public Odom As Odom_T
    Public TeamSet As SetDoubleCar_T
    Public OdomTf As OdomTf_T
    Public TmpTf As OdomTf_T '临时的Odom变换，仅改变中间层的值，不会改变Stm32里面的值

    Public ReadOnly Property CmdFlags As Flags = New Flags

    <Description("刚刚初始化")>
    Public Property Booting As Boolean = False
    <Description("初始化完成")>
    Public ReadOnly Property BootReady As Boolean = False

    <Description("通信心跳计数")>
    Public Property HeartBeats As Integer

    <Description("硬件版本")>
    Public Property Version As String

    <Description("是否使用临时坐标")>
    Public Property EnableTmpTf As Boolean = False

    Private serial_lock As New Object

    ' 是否在周期循环内请求各类数据
    Property LoopState As Boolean = True
    Property LoopOdom As Boolean = False '2021-03-02 Odom合并到State中，但是该协议仍然有效
    Property LoopVel As Boolean = True
    Property LoopSpin As Boolean = False
    Property LoopShift As Boolean = False
    Property NetErrCnt As UInteger = 0
    <Description("状态刷新计数")>
    ReadOnly Property StateUpdateCnt As UInteger = 0
    Public Sub RefreshStateUpdateCnt()
        _StateUpdateCnt += 1
        If _StateUpdateCnt >= 10000 Then
            _StateUpdateCnt = 0
        End If
    End Sub

    Enum RecvState
        WaitHead
        WaitID
        WaitLen
        WaitData
    End Enum

    Public Sub New()
        ReDim RobotState.angles(3)
        ReDim RobotState.height(3)
        ReDim RobotState.angleErr(3)
        ReDim RobotState.rfid_data(7)
        TmpTf.dx = 0
        TmpTf.dy = 0
        TmpTf.dyaw = 0
    End Sub

    Public Sub RunWatchDog()
        Dim last_heart_beat As Integer = 0
        Task.Run(Sub()
                     While True
                         last_heart_beat = HeartBeats
                         Thread.Sleep(1000)
                         If Ready Then
                             If HeartBeats = last_heart_beat Then
                                 Console.WriteLine("Heart Beat is not Updating, Maybe Communication to Low Lowel Driver is Aborted.")
                             End If
                         End If
                     End While
                 End Sub)
    End Sub

    Public Sub SetNormalMove()
        If LoopSpin Then
            LoopSpin = False
            Spin.wheel_vel = 0
        End If
        If LoopShift Then
            LoopShift = False
            Shift.v = 0
        End If
        LoopVel = True
    End Sub

    Public Sub SetShiftMove()
        If LoopVel Then
            LoopVel = False
            CarVel.v_angular_z = 0
            CarVel.v_linear_x = 0
            CarVel.v_linear_y = 0
        End If
        If LoopSpin Then
            LoopSpin = False
            Spin.wheel_vel = 0
        End If
        LoopShift = True
    End Sub

    Public Sub SetSpinMove()
        If LoopVel Then
            LoopVel = False
            CarVel.v_angular_z = 0
            CarVel.v_linear_x = 0
            CarVel.v_linear_y = 0
        End If
        If LoopShift Then
            LoopShift = False
            Shift.v = 0
        End If
        LoopSpin = True
    End Sub

    Public Sub HeartBeating()
        HeartBeats += 1
        If HeartBeats > 10000 Then
            HeartBeats = 0
        End If
    End Sub

    Public Function WaitAck(msg_id As Byte) As Boolean

        Dim read = Function(rd_len As Integer, id As Byte) As Boolean
                       Dim byte_v(rd_len - 1) As Byte
                       Me.Read(byte_v, 0, rd_len)
                       'print_bytes(byte_v)
                       Return Parse(byte_v, id)
                   End Function

        Dim cnt = 0
        Dim time_out = False

        Dim _t = New Timers.Timer(200) With {
            .Enabled = True,
            .AutoReset = False
            }
        AddHandler _t.Elapsed, Sub()
                                   Console.WriteLine("[ERROR] Read time out")
                                   DiscardInBuffer()
                                   time_out = True
                               End Sub

        While BytesToRead < AckLength(msg_id)
            If time_out Then
                _t.Dispose()
                Console.WriteLine("Send Message But No Ack Get")
                Return False
            End If
        End While
        Dim ret = read(BytesToRead, msg_id)
        _t.Dispose()
        Return ret
    End Function

    ''' <summary>
    ''' 下发电机控制指令到下位机
    ''' </summary>
    Public Sub SetEachMotor()
        SyncLock serial_lock
            SendMsg(GenerateMsg(MsgType.SetEachMotor, EachMotor))
        End SyncLock
    End Sub

    Private robot_state_task As Task
    Public Sub RequestRobotState()

        Dim robot_state_len = Marshal.SizeOf(RobotState) + 5
        Dim robot_odom_len = Marshal.SizeOf(Odom) + 5
        Const period As Integer = 1
        If CmdFlags.PollingState Then
            Console.WriteLine("Already Polling, Please Wait Current Task Finish.")
            Return
        End If
        robot_state_task = Task.Factory.StartNew(Sub()
                                                     _CmdFlags.PollingState = True
                                                     Console.WriteLine("[Task] request state start.")
                                                     While Ready
                                                         Try
                                                             LoopVersion()
                                                             If LoopState Then
                                                                 If SendMsg(FrameGetRobotState) Then
                                                                     RefreshStateUpdateCnt()
                                                                     If Not BootReady Then
                                                                         _BootReady = True
                                                                         Booting = True
                                                                     End If
                                                                 End If
                                                             End If
                                                             If LoopOdom Then SendMsg(FrameGetOdom)
                                                             If LoopVel Then
                                                                 SendMsg(GenerateMsg(MsgType.SetVelocity, CarVel))
                                                             ElseIf LoopSpin Then
                                                                 SendMsg(GenerateMsg(MsgType.SetSpin, Spin))
                                                             ElseIf LoopShift Then
                                                                 SendMsg(GenerateMsg(MsgType.SetShift, Shift))
                                                             End If
                                                             LoopArms()
                                                             LoopClear()
                                                             LoopRaise()
                                                             LoopOdomTf()
                                                             LoopDoubleCar()

                                                             If NetErrCnt >= 10 Then
                                                                 SendMsg(GenerateMsg(MsgType.SetVelocity, New SetVelocity_T()))
                                                                 Console.WriteLine("[Fatal] Long Time No Ack From Board, Stop Move")
                                                                 'NetErrCnt = 0
                                                             End If
                                                             Thread.Sleep(period)
                                                         Catch ex As Exception
                                                             Console.WriteLine($"[Error] {ex.Message}")
                                                             DiscardInBuffer()
                                                         End Try
                                                     End While
                                                     Console.WriteLine("[Task] Exit Task.")
                                                     _CmdFlags.PollingState = False
                                                 End Sub, TaskCreationOptions.LongRunning)
    End Sub

    ''' <summary>
    ''' 控制整车速度
    ''' </summary>
    ''' <param name="vx">前进方向速度</param>
    ''' <param name="vy">右方速度</param>
    ''' <param name="rz">角速度</param>
    Public Sub SetVelocity(vx As Short, vy As Short, rz As Short)
        SetNormalMove()
        CarVel.v_linear_x = vx
        CarVel.v_linear_y = vy
        CarVel.v_angular_z = rz
        'Debug.Print($"Set Vx:{vx / 1000.0}, Vy:{vy / 1000.0}, Vth:{rz / 1000.0}")
    End Sub

    Public Sub SetShift(angle As Double, v As Short)
        SetShiftMove()
        Shift.angle = angle
        Shift.v = v
    End Sub

    Public Sub SetSpin(v As Short)
        SetSpinMove()
        If Math.Abs(v) > 500 Then
            Console.WriteLine("Too Big Spin Speed")
            Return
        End If
        Spin.wheel_vel = v
    End Sub

    ''' <summary>
    ''' 控制左右抱臂状态
    ''' </summary>
    Public Sub SetArms()
        _CmdFlags.SetArm = True
    End Sub

    Public Sub LoopArms()
        If CmdFlags.SetArm Then
            _CmdFlags.SetArm = False
            SendMsg(GenerateMsg(MsgType.SetHydroArm, Arm))
        End If
    End Sub

    ''' <summary>
    ''' 控制举升
    ''' </summary>
    Public Sub SetRaise()
        _CmdFlags.SetRaise = True
    End Sub

    Public Sub LoopRaise()
        If CmdFlags.SetRaise Then
            _CmdFlags.SetRaise = False
            SendMsg(GenerateMsg(MsgType.SetHydroRaise, Raise))
        End If
    End Sub

    ''' <summary>
    ''' 请求硬件版本
    ''' </summary>
    Public Sub RequestVersion()
        _CmdFlags.GetVersion = True
    End Sub

    Public Sub LoopVersion()
        If CmdFlags.GetVersion Then
            _CmdFlags.GetVersion = False
            Debug.Print("Request Version")
            SendMsg(FrameGetVersion)
        End If
    End Sub

    Public Function ResetWheels() As Boolean
        SyncLock serial_lock
            SendMsg(FrameResetWheels)
        End SyncLock
        Return True
    End Function

    ''' <summary>
    ''' 清空缓存的值
    ''' </summary>
    Public Sub ClearRobotStateOdom()
        RobotState.odom.x = 0
        RobotState.odom.y = 0
        RobotState.odom.yaw = 0
    End Sub

    Public Sub ClearOdom()
        _CmdFlags.ClearOdom = True
    End Sub

    Public Sub LoopClear()
        If CmdFlags.ClearOdom Then
            _CmdFlags.ClearOdom = False
            SendMsg(FrameClearOdom)
        End If
    End Sub

    Public Sub SetDoubleCar(enable As Boolean, position As String, distance As UInteger)
        TeamSet.enable = IIf(enable, 1, 0)
        TeamSet.position = TeamPosition(position)
        TeamSet.distance = distance
        _CmdFlags.SetDoubleCar = True
    End Sub

    Public Sub LoopDoubleCar()
        If CmdFlags.SetDoubleCar Then
            _CmdFlags.SetDoubleCar = False
            SendMsg(GenerateMsg(MsgType.SetDoubleCar, TeamSet))
        End If
    End Sub

    Public Sub SetOdomTf(dx As Single, dy As Single, dth As Single, Optional method As Byte = SetOdomMethod.Incremental)
        OdomTf.method = method
        OdomTf.dx = dx
        OdomTf.dy = dy
        OdomTf.dyaw = dth
        _CmdFlags.SetOdomTf = True
    End Sub

    Public Sub LoopOdomTf()
        If CmdFlags.SetOdomTf Then
            _CmdFlags.SetOdomTf = False
            SendMsg(GenerateMsg(MsgType.SetOdomTf, OdomTf))
        End If
    End Sub

    Public Sub UseTmpTf()
        RobotState.odom.x = (TmpTf.dx * 1000.0)
        RobotState.odom.y = (TmpTf.dy * 1000.0)
        RobotState.odom.yaw = TmpTf.dyaw
    End Sub

    Public Sub PerformTmpTf()
        Dim dx = TmpTf.dx
        Dim dy = TmpTf.dy
        Dim dth = TmpTf.dyaw
        SetOdomTf(dx, dy, dth, method:=SetOdomMethod.Absolute)
    End Sub

    Public Sub TmpClearOdom()
        _CmdFlags.TmpClearOdom = True
    End Sub

    ''' <summary>
    ''' 仅在内部使用
    ''' </summary>
    Private Sub _TmpClearOdom()
        If CmdFlags.TmpClearOdom Then
            _CmdFlags.TmpClearOdom = False
            TmpTf.dx = 0
            TmpTf.dy = 0
            TmpTf.dyaw = 0
        End If
    End Sub

    ''' <summary>
    ''' 临时更新坐标
    ''' </summary>
    ''' <param name="x">激光x</param>
    ''' <param name="y">激光y</param>
    ''' <param name="th">激光th</param>
    Public Sub TmpSetOdomTf(x As Double, y As Double, th As Double)
        TmpTf.dx = x
        TmpTf.dy = y
        TmpTf.dyaw = th
    End Sub

    Public Sub ActiveTmpTf()
        EnableTmpTf = True
    End Sub

    Public Sub RecoverTmpTf()
        EnableTmpTf = False
    End Sub

    Public Sub SetRobotStateOdom(x As Single, y As Single, yaw As Single)
        RobotState.odom.x = x * 1000.0
        RobotState.odom.y = y * 1000.0
        RobotState.odom.yaw = yaw
    End Sub

    ''' <summary>
    ''' 解析协议
    ''' </summary>
    ''' <param name="frame"></param>
    ''' <param name="id">发送的时候的ID</param>
    ''' <returns>失败返回false</returns>
    Private Function Parse(frame As Byte(), id As Byte) As Boolean
        Dim ret As Boolean = False
        Dim total_len = frame.Length '总长度
        Dim parsed_len = 0           '已解析的长度
        Dim left_len = total_len     '未解析的长度
        Dim tmp_buf(0) As Byte
        Dim print_buf = frame        '用于打印的buf

        Dim parse_to_object = Function(st As Object) As Object
                                  Dim data_len = Marshal.SizeOf(st)
                                  Dim data_bytes(data_len) As Byte
                                  Array.Copy(frame, 3, data_bytes, 0, data_len)
                                  Return Transport.ByteToStruct(data_bytes, st.GetType())
                              End Function

        ' 把角度限定在-180~180之间
        Dim reset_angle = Function(yaw As Single) As Single
                              While Math.Abs(yaw) > 180
                                  yaw = yaw - yaw / Math.Abs(yaw) * 360
                              End While
                              Return yaw
                          End Function

        If frame.Length = 0 Then
            Console.WriteLine("Empty Frame")
            Return False
        End If

        While True
            If frame(0) <> &H5A Then
                Console.WriteLine("Error Head")
                parsed_len = 1
            Else
                Dim msg_id As Byte = frame(1)
                If msg_id > ID_MESSGAE_MAX Then
                    parsed_len = 2
                Else
                    Dim len As Integer = frame(2)
                    parsed_len = len + 5

                    If frame.Length < parsed_len Then
                        Console.WriteLine("No Enough Bytes to Parse")
                        Exit While
                    End If

                    Dim crc_get As Byte = frame(len + 3)
                    Dim crc_buf(len + 2) As Byte
                    Array.Copy(frame, crc_buf, crc_buf.Length)

                    If crc_get <> Calculate.Sum(crc_buf) Then
                        Console.WriteLine("Error CRC")
                        Return False
                    End If

                    Select Case msg_id
                        Case &H0
                            Dim _version(15) As Byte
                            Dim _date(15) As Byte
                            Array.Copy(frame, 3, _version, 0, _version.Length)
                            Array.Copy(frame, 3 + _version.Length, _date, 0, _date.Length)
                            Version = Encoding.ASCII.GetString(_version).Replace(vbNullChar, "") + "-" + Encoding.ASCII.GetString(_date).Replace(vbNullChar, "")
                            Console.WriteLine($"LowLevel Server Version: {Version}")
                        Case ID_SET_EACH_MOTOR
                            Console.WriteLine("motor control success.")
                        Case ID_GET_ROBOT_STATE
                            RobotState = parse_to_object(RobotState)
                            _TmpClearOdom()
                            If EnableTmpTf Then
                                ' 如果要用TmpTf，则必须从外部持续输入坐标信息，实时性不能保证，
                                ' 不再使用该方法
                                UseTmpTf()
                            End If
                            RobotState.odom.yaw = reset_angle(RobotState.odom.yaw)
                        Case ID_GET_ODOM
                            Odom = parse_to_object(Odom)
                        Case Else
                    End Select

                    ret = msg_id.Equals(id) Or ret '有一次匹配上就算OK
                    If Not ret Then
                        Console.WriteLine($"Message Id Not Matching, it should be {id}, but get {msg_id}")
                    End If

                End If
            End If
            left_len = left_len - parsed_len
            If left_len < AckLength(id) Then
                Exit While
            End If
            ReDim tmp_buf(left_len - 1)
            Array.Copy(frame, parsed_len, tmp_buf, 0, left_len)
            frame = tmp_buf
        End While
        If Not ret Then
            Console.WriteLine("Parse Failed, Error Frame Detail: ")
            PrintBytes(print_buf)
        End If
        Return ret
    End Function

    ''' <summary>
    ''' 生成要发送的协议
    ''' </summary>
    ''' <param name="msg_id">信息类型</param>
    ''' <param name="data_struct">数据结构体</param>
    ''' <returns></returns>
    Public Function GenerateMsg(msg_id As MsgType, data_struct As Object) As Byte()
        Dim tx_buffer(4) As Byte
        Dim data_buf() As Byte = Nothing
        Dim data_len As Integer = 0
        tx_buffer(0) = &H5A

        data_len = Marshal.SizeOf(data_struct)
        data_buf = Transport.StructToByte(data_struct)
        tx_buffer(2) = data_len
        ReDim Preserve tx_buffer(data_len + 4)

        If msg_id = MsgType.SetHydroArm Then
            tx_buffer(1) = ID_SET_ARM
        ElseIf msg_id = MsgType.SetEachMotor Then
            tx_buffer(1) = ID_SET_EACH_MOTOR
        ElseIf msg_id = MsgType.SetVelocity Then
            tx_buffer(1) = ID_SET_VELOCITY
        ElseIf msg_id = MsgType.SetHydroRaise Then
            tx_buffer(1) = ID_SET_RAISE
        ElseIf msg_id = MsgType.SetShift Then
            tx_buffer(1) = ID_SET_SHIFT
        ElseIf msg_id = MsgType.SetSpin Then
            tx_buffer(1) = ID_SET_SPIN
        ElseIf msg_id = MsgType.GetRobotState Then
            tx_buffer(1) = ID_GET_ROBOT_STATE
        ElseIf msg_id = MsgType.SetDoubleCar Then
            tx_buffer(1) = ID_SET_DOUBLE_CAR
        ElseIf msg_id = MsgType.SetOdomTf Then
            tx_buffer(1) = ID_SET_ODOM_TF
        End If

        If data_buf IsNot Nothing Then
            Array.Copy(data_buf, 0, tx_buffer, 3, data_buf.Length)
        End If
        '填入固定结束符
        Dim crc As Byte = Calculate.Sum(tx_buffer)
        tx_buffer(tx_buffer.Length - 2) = crc
        tx_buffer(tx_buffer.Length - 1) = &H7E
        Return tx_buffer
    End Function

    ''' <summary>
    ''' 下发协议
    ''' </summary>
    ''' <param name="buf">协议内容</param>
    ''' <returns></returns>
    Public Function SendMsg(buf() As Byte) As Boolean
        Dim ret As Boolean = True

        If Not Ready Then
            Return False
        End If

        If buf.Length < 5 Then
            ret = False
            Console.WriteLine("[Error] Wrong Length")
        Else
            If buf(0) <> &H5A Then
                ret = False
                Console.WriteLine("[Error] Wrong Head")
            Else
                Dim total_len As Integer = buf(2) + 5
                Debug.WriteLine("Start Sending")
                Write(buf, 0, total_len)
                Dim watch = New Stopwatch()
                watch.Start()  '开始监视代码运行时间
                ret = WaitAck(buf(1))
                If ret Then
                    HeartBeating()
                End If
                watch.Stop()
                Dim time_span = watch.Elapsed
                Debug.WriteLine("Communicating Time：{0} ms", time_span.TotalMilliseconds) ' 总毫秒数

            End If
        End If

        If ret Then
            If NetErrCnt > 0 Then NetErrCnt = NetErrCnt - 1
        Else
            NetErrCnt += 1
        End If
        Return ret
    End Function

End Class

Public Class Calculate
    Shared Function Sum(ByVal buf() As Byte) As Byte
        Dim ret As Byte = 0
        Dim _sum As Integer = 0
        For Each b As Byte In buf
            _sum = (_sum + b) Mod 256
        Next
        ret = _sum
        Return ret
    End Function
End Class

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetHydroArm_T

    Public Structure Couple
        Public Structure Arm
            Public Strech As Byte
            Public DrawBack As Byte
            Public Pump As Byte
        End Structure

        Public ArmL As Arm
        Public ArmR As Arm
    End Structure

    Public Couples As Couple

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetEachMotor_T

    Public Structure Wheel
        Public VL As Short
        Public VR As Short
    End Structure

    Public WheelFL As Wheel
    Public WheelFR As Wheel
    Public WheelBL As Wheel
    Public WheelBR As Wheel

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetVelocity_T

    Public v_linear_x As Short
    Public v_linear_y As Short
    Public v_angular_z As Short

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetHydroRaise_T

    Public up As Byte
    Public down As Byte
    Public pump As Byte

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetSpin_T

    Public angular_vel As Short
    Public wheel_vel As Short

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetShift_T

    Public angle As Double
    Public v As Short

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure RobotState_T

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public angles() As Single

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public height() As Single

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=4)>
    Public angleErr() As Single

    Public small_gear As UShort
    Public big_gear As UShort
    Public wheel_mps As Single
    Public team_info As SetDoubleCar_T
    Public odom As Odom_T
    Public ctrl_mode As Byte
    Public arm_state As Byte
    Public raise_state As Byte
    Public rfid_state As Byte

    <MarshalAs(UnmanagedType.ByValArray, SizeConst:=8)>
    Public rfid_data() As Byte

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure SetDoubleCar_T

    Public enable As Byte
    Public position As Byte     '相对位置1-前车，2-后车
    Public distance As UInteger '两车中心距(mm)

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure Odom_T

    Public v_liner_x As Short      '线速度 前>0 后<0  mm/s
    Public v_liner_y As Short      '差分轮 为0        mm/s
    Public v_angular_z As Short    '角速度 左>0 右<0  0.01rad/s   100 means 1 rad/s
    Public x As Integer            '里程计坐标x       mm (这里long为4字节， 下同)
    Public y As Integer            '里程计坐标y       mm
    Public yaw As Single           '里程计航角        度

End Structure

<StructLayout(LayoutKind.Sequential, CharSet:=CharSet.Auto, Pack:=1)>
Public Structure OdomTf_T

    Public method As Byte
    Public dx As Single
    Public dy As Single
    Public dyaw As Single

End Structure


Public Structure Flags
    Public PollingState As Boolean
    Public GetVersion As Boolean
    Public SetOdomTf As Boolean
    Public ClearOdom As Boolean
    Public SetDoubleCar As Boolean
    Public SetArm As Boolean
    Public SetRaise As Boolean
    Public TmpClearOdom As Boolean
End Structure

Public Enum MsgType
    SetHydroArm
    SetHydroRaise
    SetEachMotor
    SetVelocity
    SetSpin
    SetShift
    GetRobotState
    SetDoubleCar
    SetOdomTf
End Enum
